#include "fishbot_generic_behaviors/get_geometry_msgs.hpp"

namespace fishbot
{
    namespace behaviors
    {
        static const rclcpp::Logger logger = rclcpp::get_logger("generic_behavior.GetGeometryMsg");

        GetPoseStampedMsg::GetPoseStampedMsg(const std::string &name, const BT::NodeConfiguration &config)
            : BT::SyncActionNode(name, config)
        {
        }

        BT::PortsList GetPoseStampedMsg::providedPorts()
        {
            return BT::PortsList({BT::InputPort<std::string>("frame_id"),
                                  BT::InputPort<Vector3D>("position"),
                                  BT::InputPort<Vector4D>("quaternion"),
                                  BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose_stamped")});
        }

        BT::NodeStatus GetPoseStampedMsg::tick()
        {
            // Retrieve inputs
            std::string frame_id;
            Vector3D position;
            Vector4D quaternion;

            if (!getInput("frame_id", frame_id) ||
                !getInput("position", position) ||
                !getInput("quaternion", quaternion))
            {
                RCLCPP_ERROR_STREAM(logger, "Missing input");
                return BT::NodeStatus::FAILURE;
            }

            geometry_msgs::msg::PoseStamped pose;

            pose.header.frame_id = frame_id;
            pose.pose.position.x = position.x;
            pose.pose.position.y = position.y;
            pose.pose.position.z = position.z;

            pose.pose.orientation.w = quaternion.w;
            pose.pose.orientation.x = quaternion.x;
            pose.pose.orientation.y = quaternion.y;
            pose.pose.orientation.z = quaternion.z;

            setOutput("pose_stamped", pose);

            return BT::NodeStatus::SUCCESS;
        }

        GetVector3StampedMsg::GetVector3StampedMsg(const std::string &name, const BT::NodeConfiguration &config)
            : BT::SyncActionNode(name, config)
        {
        }

        BT::PortsList GetVector3StampedMsg::providedPorts()
        {
            return BT::PortsList({
                BT::InputPort<std::string>("frame_id"),
                BT::InputPort<Vector3D>("vector"),
                BT::OutputPort<geometry_msgs::msg::Vector3Stamped>("vector_stamped"),
            });
        }

        BT::NodeStatus GetVector3StampedMsg::tick()
        {
            std::string frame_id;
            Vector3D vector;

            if (!getInput("frame_id", frame_id) ||
                !getInput("vector", vector))
            {
                RCLCPP_ERROR_STREAM(logger, "Missing input");
                return BT::NodeStatus::FAILURE;
            }

            RCLCPP_INFO(logger, "Vector: %f, %f, %f", vector.x, vector.y, vector.z);

            geometry_msgs::msg::Vector3Stamped vector_stamped;
            vector_stamped.header.frame_id = frame_id;
            vector_stamped.vector.x = vector.x;
            vector_stamped.vector.y = vector.y;
            vector_stamped.vector.z = vector.z;

            setOutput("vector_stamped", vector_stamped);
            return BT::NodeStatus::SUCCESS;
        }
    }
}